#include "mav_msg.h"
using namespace mav;
using namespace asio;
static mavio* mavio_ptrs[MAVLINK_COMM_NUM_BUFFERS];

void comm_send_ch(mavlink_channel_t channel, uint8_t byte) {
	mavio* io = mavio_ptrs[channel];
	if (io->send != NULL) {
		io->send(byte);
	}
};


mav::mavio::mavio(asio::serial_port& ser):serial(&ser), bufvec(asio::buffer(&bufchar, 1))
{
	send = [&](unsigned char buf) {
		serial->async_write_some(asio::buffer(&buf, 1), std::bind(&mavio::emptyfunc, this));
	};
	serial->async_read_some(bufvec, std::bind(&mavio::serReceivedfunc, this));
}

mav::mavio::mavio(asio::ip::udp::socket& soc):udp(&soc), bufvec(asio::buffer(&bufchar, 1024))
{
	send = [&](unsigned char buf) {
		udp->async_send(asio::buffer(&buf, 1), std::bind(&mavio::emptyfunc, this));
	};
	bufvec = asio::buffer(&bufchar, udp->available());
	udp->async_receive(bufvec,std::bind(&mavio::socReceivedfunc, this));
}


void mav::mavio::setReceivedFunc(std::function<void(unsigned char)> func)
{
	received = func;
}

mav::mavio::~mavio()
{
}

void mav::mavio::serReceivedfunc()
{
	if (received != NULL) {
		received(bufchar[0]);
	}
	serial->async_read_some(bufvec, std::bind(&mavio::serReceivedfunc, this));
}
#include<iostream>
void mav::mavio::socReceivedfunc()
{
	if (received != NULL&& bufvec.size()) {
		//std::cout <<"size:::"<< bufvec.size() << std::endl;
		for (size_t i = 0; i < bufvec.size(); i++)
		{
			received(bufchar[i]);
		}
	}
	bufvec = asio::buffer(&bufchar, udp->available());
	udp->async_receive(bufvec, std::bind(&mavio::socReceivedfunc, this));
}

void mav::bind_mavio_to(unsigned char num, mavio& mav)
{
	if (num > MAVLINK_COMM_NUM_BUFFERS) {
		throw "In mav::bind_mavio_to num is bigger than MAVLINK_COMM_NUM_BUFFERS(" + std::to_string(num) + ")";
	}
	else {
		mavio_ptrs[num] = &mav;
	}
}

mavio& mav::get_mavio(unsigned char num)
{
	if (num > MAVLINK_COMM_NUM_BUFFERS) {
		throw "In mav::bind_mavio_to num is bigger than MAVLINK_COMM_NUM_BUFFERS(" + std::to_string(num) + ")";
	}
	else {
		return *mavio_ptrs[num];
	}
}
